#ifndef __AHRS_H_
#define __AHRS_H_
#include "mpu9250.h"
#include "mymath.h"

typedef struct _position_str
{
	double lat;
	double lon;
	float h;
}position_str;

typedef struct _ahrs_str
{
		position_str origin;
		
		vector3f pos;
		vector3f vel;
	  
    float pitch,roll,yaw;
    float q0,q1,q2,q3;

    vector3f acc;
    vector3f gyro;
		vector3f mag;
		vector3f accSport;
		vector3f accElf;
    float Mat[3][3];
		int originIsSet;
		int positionIsUpdate;
		int resetSportAcc;
}ahrs_str;
extern ahrs_str attitude;

void ahrsInit(void);
void AccelpreTreat(float *Acc, vector3f *output);
void GyropreTreat(float *Gyro, vector3f *output);
void MagpreTreat(float *Mag, vector3f *output);
void q_to_angle(ahrs_str *atti);
void euler_to_q(vector3f angle,float *q);

void Bf2Ef(vector3f *input,vector3f *output);
void Ef2Bf(vector3f *input,vector3f *output);

vector3f get_descartes_position(ahrs_str *atti);

#endif

